/*Min Speed Filter for Yaw drift Correction*/
#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter

//1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only
#define OUTPUTMODE 1
#define USE_MAGNETOMETER 0  //if you have the magnetometer  //use 2100 bytes
#define PERFORMANCE_REPORTING 0
#define USE_TX_BUFFER 1 // Use Serial TX Buffer library


/* ***************************************************************************************** */
// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.23mV/ADC step
// ADXL335 Sensitivity (from datasheet) => 330mV/g, => 102 ADC steps for 1G
// saturation at +- 3g => +- 1V => 310 ADC steps
// saturation at +- 3.6g => +- 1.19V => 369 ADC steps
#define GRAVITY 102 //this equivalent to 1G in the raw data coming from the accelerometer 
//#define Accel_Scale(x) x*(GRAVITY/9.81) //Convert an acceleration (in meters for seconds square) to raw data
#define Accel_Scale(x) x*(9.81/GRAVITY) //Return the scaled ADC raw data of the acceleros in meters for seconds square

// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, => 1.03 ADC steps for 1°/s => 0.97 °/s for 1 ADC step
// saturation at +- 300°/s => +- 1V => +-310 ADC steps
#define Gyro_Gain 0.97 //Gyro gain (°/s per ADC step)
#define Gyro_Scaled(x) x*radians(Gyro_Gain) //Return the scaled ADC raw data of the gyro in radians per second

// The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets:
// We have to take this values with the IMU flat (0° roll, 0° pitch)
//511 in theory
#define acc_offset_x 510
#define acc_offset_y 502
#define acc_offset_z 507
//381 in theory
#define gyro_offset_roll 371
#define gyro_offset_pitch 372
#define gyro_offset_yaw 383

//511 in theory //for vtol
/*#define acc_offset_x 507
 #define acc_offset_y 510
 #define acc_offset_z 502
 //381 in theory
 #define gyro_offset_roll 383
 #define gyro_offset_pitch 371
 #define gyro_offset_yaw 372*/

// Gains for DCM
//avant de changer pour les acceleros
#define Kp_ROLLPITCH 0.015
#define Ki_ROLLPITCH 0.0005
//gains x 11 apres changement. a verifier
//#define Kp_ROLLPITCH 0.165
//#define Ki_ROLLPITCH 0.0055
#define Kp_YAW 0.5
#define Ki_YAW 0.0025

// Booleans
#define FALSE 0
#define TRUE 1

//Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
byte sensors[6] = {
  6,7,3,0,1,2};  // For Hardware v2 flat

//byte  sensors[6] = {
//  3,6,7,2,0,1};  // For mavion vtol

//Sensors sign: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
int SENSOR_SIGN[]={
  1,-1,-1,1,-1,1,-1,-1,-1}; //{1,-1,-1,-1,1,-1}

//int SENSOR_SIGN[]={
//  1,-1,-1,-1,-1,-1,-1,-1,-1}; // For mavion vtol



